﻿
// 2020/1/13: 首个记录
// 2020/5/15: 移除编译warning。完善VehicleSample()
// 2020/11/12: 支持Spadas7

#ifndef VEHICLE_SAMPLE_V4_H
#define VEHICLE_SAMPLE_V4_H

#include "spadas.h"

namespace vehicle_sample_v4
{
	using namespace spadas;

#if SPADAS_VERSION_MAJOR < 7
	typedef OptionalDouble GeneralElement;
#endif

	enum GearPosition
	{
		GP_Unknown = 0, // 未知
		GP_Park = 1, // P档
		GP_Reverse = 2, // R档
		GP_Neutral = 3, // N档
		GP_Drive = 4, // D档
	};

	enum TurnState
	{
		TS_Unknown = 0, // 未知
		TS_None = 1, // 未打灯
		TS_TurnLeft = 2, // 左转向灯
		TS_TurnRight = 3, // 右转向灯
		TS_Emergency = 4, // 双闪
	};

	enum Horn
	{
		H_Unknown = 0, // 未知
		H_None = 1, // 不响
		H_Active = 2, // 响
	};

	enum HeadLight
	{
		HL_Unknown = 0, // 未知
		HL_None = 1, // 未激活
		HL_Near = 2, // 近光
		HL_Far = 3, // 远光
	};

	enum Wiper
	{
		W_Unknown = 0, // 未知
		W_None = 1, // 未激活
		W_Single = 2, // 单次
		W_Slow1 = 3, // 慢 1
		W_Slow2 = 4, // 慢 2
		W_Slow3 = 5, // 慢 3
		W_Slow4 = 6, // 慢 4
		W_Slow5 = 7, // 慢 5
		W_Medium = 8, // 中档
		W_Fast = 9, // 快档
	};

	struct VehicleSample
	{
		GlobalTimestamp timeStamp; //时间戳

		Double speed; // 本车车速 kph
		OptionalDouble yawRate; // 横摆角速度 deg/s
		OptionalDouble curvature; // 行驶曲率 1/m 左转为正 未来路径: y=(curvature/2)*(x^2)+(curvature)*(frontOverhang+wheelBase)*x
		Double kilometerAge;    //总里程数  km
		OptionalDouble accelX; // 本车纵向加速度 m/s2
		OptionalDouble accelY; // 本车横向加速度 m/s2
		OptionalDouble jerkX; // 本车纵向急动度 m/s3
		OptionalDouble jerkY; // 本车横向急动度 m/s3
		Array<Point2D> trajectory; // 历史路径
		Array<Point2D> contour; // 轮廓描述，以(0,0)开始，以(0,0)结束

		Double vehicleWidth; // 车宽 m
		Double vehicleLength; // 车长 m
		Double vehicleHeight; // 车高 m
		Double steerAngleRatio; // 传动比
		Double wheelBase; // 轴距 m
		Double rearTread; // 轮距 m
		Double frontOverhang; //前悬 m

		OptionalDouble steerAngle; // 方向盘转角 deg
		OptionalDouble steerAngleRate; // 方向盘转角速度 deg/s
		OptionalDouble steerTorque; // 方向盘力矩 Nm

		OptionalDouble brake; // 刹车位置 %
		OptionalDouble throttle; // 油门位置 %
		GearPosition gear; // 档位
		TurnState turn; // 转向灯
		Horn horn; // 喇叭
		HeadLight headLight; // 大灯
		Wiper wiper; // 雨刮

		OptionalDouble flSpeed; // 左前轮轮速 kph
		OptionalDouble frSpeed; // 右前轮轮速 kph
		OptionalDouble rlSpeed; // 左后轮轮速 kph
		OptionalDouble rrSpeed; // 右后轮轮速 kph

		OptionalDouble engineSpeed; // 引擎转速 rpm
		OptionalDouble engineTorque; // 引擎扭矩 Nm

		OptionalDouble frontEndLateralSpeed; // 前保中心横向速度 m/s
		OptionalDouble rearEndLateralSpeed; // 后保中心横向速度 m/s

		VehicleSample()
		{
			vehicleWidth = 1.9;
			vehicleLength = 4.6;
			vehicleHeight = 1.5;
			steerAngleRatio = 15;
			wheelBase = 2.8;
			rearTread = 1.6;
			frontOverhang = 0.9;
			gear = GP_Unknown;
			turn = TS_Unknown;
			horn = H_Unknown;
			headLight = HL_Unknown;
			wiper = W_Unknown;
			speed = 0;
			kilometerAge = 0;
		}

		static String getProtocolName()
		{
			return "vehicle-sample-v4";
		}

		static String getTitle()
		{
			return "Contour point count,Trajectory point count,Speed[KPH],Yaw rate[°/s],Curvature[1/m],Acceleration-x[m/s2],Acceleration-y[m/s2],Vehicle width[m],Vehicle length[m],Vehicle height[m],Steer angle ratio,Wheel base[m],Rear tread[m],Front overhang[m],Steer angle[°],Steer angle rate[°/s],Brake position[%],Throttle position[%],Gear position,Turn,Wheel speed (fl)[KPH],Wheel speed (fr)[KPH],Wheel speed (rl)[KPH],Wheel speed (rr)[KPH],Engine speed[RPM],Engine torque[Nm],Horn,Head light,Wiper,Total distance[KM],Front-end lateral speed[m/s],Rear-end lateral speed[m/s],Steer angle torque[Nm],Jerk-x[m/s3],Jerk-y[m/s3],Reserved1,Reserved2,Reserved3,Reserved4,Reserved5,Contour points...";
		}

		static Bool supportInterpolation()
		{
			return TRUE;
		}

		GeneralSample toGeneralSample()
		{
			GeneralSample sample;
			sample.timeStamp = timeStamp;
			sample.values = Array<GeneralElement>(40 + 2 * (contour.size() + trajectory.size()));

			sample.values[0] = contour.size();
			sample.values[1] = trajectory.size();
			sample.values[2] = speed;
			sample.values[3] = yawRate;
			sample.values[4] = curvature;
			sample.values[5] = accelX;
			sample.values[6] = accelY;
			sample.values[7] = vehicleWidth;
			sample.values[8] = vehicleLength;
			sample.values[9] = vehicleHeight;
			sample.values[10] = steerAngleRatio;
			sample.values[11] = wheelBase;
			sample.values[12] = rearTread;
			sample.values[13] = frontOverhang;
			sample.values[14] = steerAngle;
			sample.values[15] = steerAngleRate;
			sample.values[16] = brake;
			sample.values[17] = throttle;
			sample.values[18] = (UInt)gear;
			sample.values[19] = (UInt)turn;
			sample.values[20] = flSpeed;
			sample.values[21] = frSpeed;
			sample.values[22] = rlSpeed;
			sample.values[23] = rrSpeed;
			sample.values[24] = engineSpeed;
			sample.values[25] = engineTorque;
			sample.values[26] = (UInt)horn;
			sample.values[27] = (UInt)headLight;
			sample.values[28] = (UInt)wiper;
			sample.values[29] = kilometerAge;
			sample.values[30] = frontEndLateralSpeed;
			sample.values[31] = rearEndLateralSpeed;
			sample.values[32] = steerTorque;
			sample.values[33] = jerkX;
			sample.values[34] = jerkY;

			for (UInt i = 0; i < contour.size(); i++)
			{
				sample.values[40 + 2 * i] = contour[i].x;
				sample.values[41 + 2 * i] = contour[i].y;
			}

			UInt trajectoryBase = 40 + 2 * contour.size();
			for (UInt i = 0; i < trajectory.size(); i++)
			{
				sample.values[trajectoryBase + 2 * i] = trajectory[i].x;
				sample.values[trajectoryBase + 2 * i + 1] = trajectory[i].y;
			}

			sample.significantCount = 40 + 2 * contour.size();
			return sample;
		}

		Bool fromGeneralSample(GeneralSample sample)
		{
			return fromGeneralSample(getProtocolName(), sample);
		}

		Bool fromGeneralSample(String protocol, GeneralSample sample)
		{
			if (protocol == "vehicle-sample-v4")
			{
				if (sample.values.size() < 40) return FALSE;

				UInt contourSize = (UInt)sample.values[0].value;
				UInt trajectorySize = (UInt)sample.values[1].value;
				if (sample.values.size() == 40 + 2 * contourSize) trajectorySize = 0;
				else if (sample.values.size() != 40 + 2 * (contourSize + trajectorySize)) return FALSE;

				timeStamp = sample.timeStamp;

				speed = sample.values[2].valid ? sample.values[2].value : 0;
				yawRate = sample.values[3];
				curvature = sample.values[4];
				accelX = sample.values[5];
				accelY = sample.values[6];
				vehicleWidth = sample.values[7].valid ? sample.values[7].value : 1.9;
				vehicleLength = sample.values[8].valid ? sample.values[8].value : 4.6;
				vehicleHeight = sample.values[9].valid ? sample.values[9].value : 1.5;
				steerAngleRatio = sample.values[10].valid ? sample.values[10].value : 15;
				wheelBase = sample.values[11].valid ? sample.values[11].value : 2.8;
				rearTread = sample.values[12].valid ? sample.values[12].value : 1.6;
				frontOverhang = sample.values[13].valid ? sample.values[13].value : 0.9;
				steerAngle = sample.values[14];
				steerAngleRate = sample.values[15];
				brake = sample.values[16];
				throttle = sample.values[17];
				gear = sample.values[18].valid ? (GearPosition)(UInt)sample.values[18].value : GP_Unknown;
				turn = sample.values[19].valid ? (TurnState)(UInt)sample.values[19].value : TS_Unknown;
				flSpeed = sample.values[20];
				frSpeed = sample.values[21];
				rlSpeed = sample.values[22];
				rrSpeed = sample.values[23];
				engineSpeed = sample.values[24];
				engineTorque = sample.values[25];
				horn = sample.values[26].valid ? (Horn)(UInt)sample.values[26].value : H_Unknown;
				headLight = sample.values[27].valid ? (HeadLight)(UInt)sample.values[27].value : HL_Unknown;
				wiper = sample.values[28].valid ? (Wiper)(UInt)sample.values[28].value : W_Unknown;
				kilometerAge = sample.values[29].value;
				frontEndLateralSpeed = sample.values[30].value;
				rearEndLateralSpeed = sample.values[31].value;
				steerTorque = sample.values[32].value;
				jerkX = sample.values[33].value;
				jerkY = sample.values[34].value;

				contour = Array<Point2D>(contourSize);
				for (UInt i = 0; i < contourSize; i++)
				{
					contour[i].x = (Float)sample.values[40 + 2 * i].value;
					contour[i].y = (Float)sample.values[41 + 2 * i].value;
				}

				trajectory = Array<Point2D>(trajectorySize);
				UInt trajectoryBase = 40 + 2 * contourSize;
				for (UInt i = 0; i < trajectorySize; i++)
				{
					trajectory[i].x = (Float)sample.values[trajectoryBase + 2 * i].value;
					trajectory[i].y = (Float)sample.values[trajectoryBase + 2 * i + 1].value;
				}

				return TRUE;
			}
			else if (protocol == "vehicle-sample-v3")
			{
				if (sample.values.size() < 27) return FALSE;

				UInt trajectorySize = (UInt)sample.values[0].value;
				if (sample.values.size() == 27) trajectorySize = 0;
				else if (sample.values.size() != 27 + 2 * trajectorySize) return FALSE;

				timeStamp = sample.timeStamp;

				speed = sample.values[1].valid ? sample.values[1].value : 0;
				yawRate = sample.values[2];
				curvature = sample.values[3];
				accelX = sample.values[4];
				accelY = sample.values[5];
				vehicleWidth = sample.values[6].valid ? sample.values[6].value : 1.9;
				vehicleLength = sample.values[7].valid ? sample.values[7].value : 4.6;
				steerAngleRatio = sample.values[8].valid ? sample.values[8].value : 15;
				wheelBase = sample.values[9].valid ? sample.values[9].value : 2.8;
				rearTread = sample.values[10].valid ? sample.values[10].value : 1.6;
				frontOverhang = sample.values[11].valid ? sample.values[11].value : 0.9;
				steerAngle = sample.values[12];
				brake = sample.values[13];
				throttle = sample.values[14];
				gear = sample.values[15].valid ? (GearPosition)(UInt)sample.values[15].value : GP_Unknown;
				turn = sample.values[16].valid ? (TurnState)(UInt)sample.values[16].value : TS_Unknown;
				flSpeed = sample.values[17];
				frSpeed = sample.values[18];
				rlSpeed = sample.values[19];
				rrSpeed = sample.values[20];
				engineSpeed = sample.values[21];
				engineTorque = sample.values[22];
				horn = sample.values[23].valid ? (Horn)(UInt)sample.values[23].value : H_Unknown;
				headLight = sample.values[24].valid ? (HeadLight)(UInt)sample.values[24].value : HL_Unknown;
				wiper = sample.values[25].valid ? (Wiper)(UInt)sample.values[25].value : W_Unknown;
				kilometerAge = sample.values[26].value;

				trajectory = Array<Point2D>(trajectorySize);
				for (UInt i = 0; i < trajectorySize; i++)
				{
					trajectory[i].x = (Float)sample.values[27 + 2 * i].value;
					trajectory[i].y = (Float)sample.values[27 + 2 * i + 1].value;
				}

				return TRUE;
			}

			return FALSE;
		}

		static VehicleSample interpolate(VehicleSample& s1, Double w1, VehicleSample& s2, Double w2, GlobalTimestamp time)
		{
			VehicleSample output;
			output.timeStamp = time;

			output.speed = s1.speed * w1 + s2.speed * w2;
			output.kilometerAge = s1.kilometerAge * w1 + s2.kilometerAge * w2;
			output.yawRate = s1.yawRate * w1 + s2.yawRate * w2;
			output.curvature = s1.curvature * w1 + s2.curvature * w2;
			output.accelX = s1.accelX * w1 + s2.accelX * w2;
			output.accelY = s1.accelY * w1 + s2.accelY * w2;
			output.jerkX = s1.jerkX * w1 + s2.jerkX * w2;
			output.jerkY = s1.jerkY * w1 + s2.jerkY * w2;
			output.vehicleWidth = s1.vehicleWidth;
			output.vehicleLength = s1.vehicleLength;
			output.vehicleHeight = s1.vehicleHeight;
			output.frontOverhang = s1.frontOverhang;
			output.rearTread = s1.rearTread;
			output.steerAngleRatio = s1.steerAngleRatio;
			output.wheelBase = s1.wheelBase;
			output.steerAngle = s1.steerAngle * w1 + s2.steerAngle * w2;
			output.steerAngleRate = s1.steerAngleRate * w1 + s2.steerAngleRate * w2;
			output.steerTorque = s1.steerTorque * w1 + s2.steerTorque * w2;
			output.brake = s1.brake * w1 + s2.brake * w2;
			output.throttle = s1.throttle * w1 + s2.throttle * w2;
			output.gear = w1 > w2 ? s1.gear : s2.gear;
			output.turn = w1 > w2 ? s1.turn : s2.turn;
			output.horn = w1 > w2 ? s1.horn : s2.horn;
			output.headLight = w1 > w2 ? s1.headLight : s2.headLight;
			output.wiper = w1 > w2 ? s1.wiper : s2.wiper;
			output.flSpeed = s1.flSpeed * w1 + s2.flSpeed * w2;
			output.frSpeed = s1.frSpeed * w1 + s2.frSpeed * w2;
			output.rlSpeed = s1.rlSpeed * w1 + s2.rlSpeed * w2;
			output.rrSpeed = s1.rrSpeed * w1 + s2.rrSpeed * w2;
			output.engineSpeed = s1.engineSpeed * w1 + s2.engineSpeed * w2;
			output.engineTorque = s1.engineTorque * w1 + s2.engineTorque * w2;
			output.frontEndLateralSpeed = s1.frontEndLateralSpeed * w1 + s2.frontEndLateralSpeed * w2;
			output.rearEndLateralSpeed = s1.rearEndLateralSpeed * w1 + s2.rearEndLateralSpeed * w2;
			output.trajectory = w1 > w2 ? s1.trajectory : s2.trajectory;
			output.contour = s1.contour;

			return output;
		}
	};
}

#endif
